#include "mpc.h"
#include <iostream>
#include <yaml-cpp/yaml.h>
#include <chrono>
#include <string>
#include "vehicle_lateral3dis.h"
#include "kf_lateral.h"
using namespace LinearMpc;
namespace auto_ros
{
namespace control
{
class VehicleLatera3dislMpc
{
public:
	VehicleLatera3dislMpc(const std::string dyn_config_filename, const std::string mpc_config_filename,
						  const std::string kf_config_filename, double rate);
	bool step(const Eigen::VectorXd &x0, const Eigen::VectorXd &u_fb,
			  const Eigen::MatrixXd &x_ref, const Eigen::MatrixXd &u_ref, const Eigen::MatrixXd &distur_ref,
			  double speed, const Eigen::MatrixXd &q, const Eigen::MatrixXd &r, double tire_f_alpha_w,
			  double tire_r_alpha_w);
	bool step(const Eigen::VectorXd &x0, const Eigen::VectorXd &u_fb, double kappa, double speed); //q r weighting factor will regulate auto based on yaml file
	bool reset_kf();
	bool reset_kf(const Eigen::VectorXd &state_esti);
	std::shared_ptr<Mpc> vehicle_mpc_ptr_;
	std::shared_ptr<VehicleLateral3dis> vehicle_dyn_ptr_;
	std::shared_ptr<DiscreteLinearSys> dis_dyn_ptr_;
	std::shared_ptr<KfLateral> kf_lateral_ptr_;
	YAML::Node mpc_yaml();
	Eigen::VectorXd x_ref_;
	Eigen::VectorXd u_ref_;

protected:
	//YAML::Node vehicle_dyn_config_;
	YAML::Node mpc_yaml_;
	double rate_;
	YAML::Node kf_yaml_;
};

} // namespace control
} // namespace auto_ros